#include "maestro.h"
#include "compass.h"
#include "gotemp.h"
#include "gps_boat.h"
#include <gps.h>
#include <unistd.h>

int main(){
	int maestro = maestro_connect();

	int compass = compass_connect();
	fprintf(stderr, "M:%d C:%d\n", maestro, compass);

//	double temp = gotemp_read();
//	fprintf(stderr, "Temp: %f\n", temp);

	double ret_heading, ret_pitch, ret_roll, ret_temp;
	get_compass(&ret_heading, &ret_pitch, &ret_roll, &ret_temp);
	fprintf(stderr, "Heading: %f, Pitch: %f, Roll: %f, Temp: %f\n", ret_heading, ret_pitch, ret_roll, ret_temp);


	gps_connect();

	double desiredHeading = 270.0;
	fprintf(stderr, "Desired heading is: %d\n");
	while(1)
	{	
		
		usleep(5000);//0);
		/*gps_data_t newData;
		int ret = gps_fetch(&newData);
		fprintf(stderr, "GPS:  ");
		if (ret == 1)
		{
			fprintf(stderr, "%d %5.5f %.7f", newData.fix.mode, newData.fix.time, newData.fix.latitude);
		}
		fprintf(stderr, "\n");
		*/
		get_compass(&ret_heading, &ret_pitch, &ret_roll, &ret_temp);
		fprintf(stderr, "Heading: %f, Pitch: %f, Roll: %f, Temp: %f\n", ret_heading, ret_pitch, ret_roll, ret_temp);

	//	temp = gotemp_read();
	//	fprintf(stderr, "Temp: %f\n", temp);


		unsigned int rudder = 0;

		double difference = desiredHeading - ret_heading;
		if (difference < -180)
			difference = difference + 360;
		if (difference > 180)
			difference = difference - 360;

		fprintf(stderr, "%f desired, %f actual, %f difference\n", desiredHeading, ret_heading, difference);
		//usleep(500000);	
		if (abs(difference) < 10)
		{
			rudder = 0;
			fprintf(stderr, "Approximately facing heading. %f desired, %f actual\n", desiredHeading, ret_heading);
		}
		else
		{
			
			rudder = 1500 - 5*difference;
		}


		fprintf(stderr, "Setting rudder (chan 5) pos: %d\n", rudder);
		setTarget(maestro, 5, rudder);
		fprintf(stderr, "Setting throttle 1500: %d\n", 1500);
		setTarget(maestro, 4, 1500);

		// [0 360] to [1000 2000]
		// * 3 + 1000
//		double sum = 4*(-ret_heading*3 + 2000);
//		unsigned int map = (unsigned int) sum;
//		fprintf(stderr, "Setting pos 4: %d\n", map);
//		setTarget(maestro, 4, map);

		// [-180 180] to [1000 2000]
	/*	usleep(1000000);
		unsigned int map2 = 4*(ret_pitch*3 + 1500);
		fprintf(stderr, "Setting pos 5: %d\n", map2);
		setTarget(maestro, 5, map2);
	*/
		fprintf(stderr, "Made it here\n");
		//fprintf(stderr, "Getting pos 4: %d\n", getPosition(maestro, 4));
		//fprintf(stderr, "Getting pos 5: %d\n", getPosition(maestro, 5));
		//fprintf(stderr, "Errors: %d\n", getErrors(maestro));
	}
	
	return 0;

}
